{
 "metadata": {
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7-final"
  },
  "orig_nbformat": 2,
  "kernelspec": {
   "name": "python3",
   "display_name": "Python 3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2,
 "cells": [
  {
   "source": [
    "# 05 使用towr规划器和逆动力学控制器\n",
    "\n",
    "用towr规划器，结合逆动力学控制器\n"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "source": [
    "重复加载机器人的过程"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
    "\n",
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator,\n",
    "    # add for ground model\n",
    "    RigidTransform, CoulombFriction, HalfSpace,\n",
    "    # add for single trunk model\n",
    "    GeometryFrame, Box, Sphere, GeometryInstance, MakePhongIllustrationProperties\n",
    ")\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "source": [
    "导入控制器和规划器"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "sys.path.append(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/anymal_control\")\n",
    "\n",
    "from controllers import MPTCController, IDController, BasicController\n",
    "\n",
    "from planners import BasicTrunkPlanner, TowrTrunkPlanner"
   ]
  },
  {
   "source": [
    "仿真过程的设置变量"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "use_lcm = False\n",
    "\n",
    "sim_time = 60.0\n",
    "dt = 5e-3\n",
    "target_realtime_rate = 1.0"
   ]
  },
  {
   "source": [
    "加载机器人和地面，同时加载towr规划模块和逆动力学控制器模块。\n",
    "\n",
    "为了使用towr规划器，需要对towr子文件夹进行编译，保存在build子文件夹中。\n",
    "这样可不安装，在python程序towr.py中启动一个子进程运行c++程序（trunk_mpc）。\n",
    "\n"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6000...\nYou can open the visualizer by visiting the following URL:\nhttp://127.0.0.1:7000/static/\nConnected to meshcat-server.\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "c467cdbf002e4d71bc1139067e772f0e"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fl_to_thigh_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "8b3658e229a04fec9257e879ce123fdd"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fl_to_knee_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "97958cf148b0471ebf5c372701f7bce4"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "c1a9d45377524adea78c9a21ef77a2e3"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fr_to_thigh_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "5b8cdea3159e422eb49ec97c8e9f7d6d"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fr_to_knee_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "3fd176b8fbd14e6ca5cc954461de2525"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "f31020b02482435d9c0edf10df51c20a"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hl_to_thigh_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "98e2f0564dbc43729547ebd7fc03bffc"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hl_to_knee_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "4ffae483488441f484741906d56d18d7"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "fd2c4e324e2f463eab2284fb7d5b5249"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hr_to_thigh_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "e8a197a7bac94b7f9f6427622c6feb8f"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hr_to_knee_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "35d80e9b78de475682b334d597300335"
      }
     },
     "metadata": {}
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-3)\n",
    "\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"../models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "quad = Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "\n",
    "# Add a flat ground with friction\n",
    "X_BG = RigidTransform()\n",
    "surface_friction = CoulombFriction(\n",
    "        static_friction = 1.0,\n",
    "        dynamic_friction = 1.0)\n",
    "plant.RegisterCollisionGeometry(\n",
    "        plant.world_body(),      # the body for which this object is registered\n",
    "        X_BG,                    # The fixed pose of the geometry frame G in the body frame B\n",
    "        HalfSpace(),             # Defines the geometry of the object\n",
    "        \"ground_collision\",      # A name\n",
    "        surface_friction)        # Coulomb friction coefficients\n",
    "plant.RegisterVisualGeometry(\n",
    "        plant.world_body(),\n",
    "        X_BG,\n",
    "        HalfSpace(),\n",
    "        \"ground_visual\",\n",
    "        np.array([0.5,0.5,0.5,0.0]))    # Color set to be completely transparent\n",
    "\n",
    "# Turn off gravity\n",
    "#g = plant.mutable_gravity_field()\n",
    "#g.set_gravity_vector([0,0,0])\n",
    "\n",
    "plant.Finalize()\n",
    "assert plant.geometry_source_is_registered()\n",
    "\n",
    "####\n",
    "trunk_frame = GeometryFrame(\"trunk\")\n",
    "trunk_frame_ids = {\"trunk\":trunk_frame.id()}\n",
    "\n",
    "for foot in [\"lf\",\"rf\",\"lh\",\"rh\"]:\n",
    "    foot_frame = GeometryFrame(foot)\n",
    "    trunk_frame_ids[foot] = foot_frame.id()\n",
    "\n",
    "####\n",
    "####\n",
    "planner = builder.AddSystem(TowrTrunkPlanner(trunk_frame_ids))\n",
    "\n",
    "controller = builder.AddSystem(IDController(plant,dt,use_lcm=use_lcm))\n",
    "\n",
    "# Connect the trunk-model planner to the controller\n",
    "builder.Connect(planner.GetOutputPort(\"trunk_trajectory\"), controller.get_input_port(1))\n",
    "\n",
    "# Connect the controller to the simulated plant\n",
    "builder.Connect(controller.GetOutputPort(\"quad_torques\"),\n",
    "                plant.get_actuation_input_port(quad))\n",
    "builder.Connect(plant.get_state_output_port(),\n",
    "                controller.GetInputPort(\"quad_state\"))\n",
    "\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "diagram_context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, diagram_context);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ],
      "image/svg+xml": "<svg height=\"583pt\" viewBox=\"0.00 0.00 1475.00 583.00\" width=\"1475pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 579)\">\n<title>_93882194770800</title>\n<polygon fill=\"white\" points=\"-4,4 -4,-579 1471,-579 1471,4 -4,4\" stroke=\"transparent\"/>\n<g class=\"cluster\" id=\"clust1\">\n<title>cluster93882194770800diagram</title>\n<polygon fill=\"none\" points=\"8,-8 8,-567 1459,-567 1459,-8 8,-8\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"733.5\" y=\"-551.8\">93882194770800</text>\n</g>\n<g class=\"cluster\" id=\"clust4\">\n<title>cluster93882194770800subsystems</title>\n<polygon fill=\"none\" points=\"16,-16 16,-536 1451,-536 1451,-16 16,-16\" stroke=\"white\"/>\n</g>\n<!-- 93882169739536 -->\n<g class=\"node\" id=\"node1\">\n<title>93882169739536</title>\n<polygon fill=\"none\" points=\"24,-113 24,-527 655,-527 655,-113 24,-113\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-511.8\">plant</text>\n<polyline fill=\"none\" points=\"24,-504 655,-504 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-467.8\">geometry_query</text>\n<polyline fill=\"none\" points=\"24,-439 275,-439 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-402.8\">WorldModelInstance_actuation</text>\n<polyline fill=\"none\" points=\"24,-374 275,-374 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-337.8\">DefaultModelInstance_actuation</text>\n<polyline fill=\"none\" points=\"24,-309 275,-309 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-272.8\">mini_cheetah_actuation</text>\n<polyline fill=\"none\" points=\"24,-244 275,-244 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-207.8\">applied_generalized_force</text>\n<polyline fill=\"none\" points=\"24,-179 275,-179 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-142.3\">applied_spatial_force</text>\n<polyline fill=\"none\" points=\"275,-113 275,-504 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-488.8\">geometry_pose</text>\n<polyline fill=\"none\" points=\"275,-481 655,-481 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-465.8\">continuous_state</text>\n<polyline fill=\"none\" points=\"275,-458 655,-458 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-442.8\">body_poses</text>\n<polyline fill=\"none\" points=\"275,-435 655,-435 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-419.8\">spatial_velocities</text>\n<polyline fill=\"none\" points=\"275,-412 655,-412 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-396.8\">spatial_accelerations</text>\n<polyline fill=\"none\" points=\"275,-389 655,-389 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-373.8\">generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-366 655,-366 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-350.8\">WorldModelInstance_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-343 655,-343 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-327.8\">WorldModelInstance_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-320 655,-320 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-304.8\">DefaultModelInstance_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-297 655,-297 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-281.8\">DefaultModelInstance_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-274 655,-274 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-258.8\">mini_cheetah_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-251 655,-251 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-235.8\">mini_cheetah_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-228 655,-228 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-212.8\">WorldModelInstance_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-205 655,-205 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-189.8\">DefaultModelInstance_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-182 655,-182 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-166.8\">mini_cheetah_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-159 655,-159 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-143.8\">reaction_forces</text>\n<polyline fill=\"none\" points=\"275,-136 655,-136 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-120.8\">contact_results</text>\n</g>\n<!-- 93882169716944 -->\n<g class=\"node\" id=\"node2\">\n<title>93882169716944</title>\n<polygon fill=\"none\" points=\"851.5,-458.5 851.5,-527.5 1085.5,-527.5 1085.5,-458.5 851.5,-458.5\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"968.5\" y=\"-512.3\">scene_graph</text>\n<polyline fill=\"none\" points=\"851.5,-504.5 1085.5,-504.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"898.5\" y=\"-477.8\">plant_pose</text>\n<polyline fill=\"none\" points=\"945.5,-458.5 945.5,-504.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1015.5\" y=\"-489.3\">lcm_visualization</text>\n<polyline fill=\"none\" points=\"945.5,-481.5 1085.5,-481.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1015.5\" y=\"-466.3\">query</text>\n</g>\n<!-- 93882169739536&#45;&gt;93882169716944 -->\n<g class=\"edge\" id=\"edge1\">\n<title>93882169739536:y0-&gt;93882169716944:u0</title>\n<path d=\"M655,-493C738.48,-493 761.58,-481.96 840.11,-481.06\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"840.52,-484.56 850.5,-481 840.48,-477.56 840.52,-484.56\" stroke=\"black\"/>\n</g>\n<!-- 93882193386624 -->\n<g class=\"node\" id=\"node4\">\n<title>93882193386624</title>\n<polygon fill=\"none\" points=\"691,-241.5 691,-310.5 1246,-310.5 1246,-241.5 691,-241.5\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"968.5\" y=\"-295.3\">controllers.inverse_dynamics_controller.IDController@00005562a6c99c80</text>\n<polyline fill=\"none\" points=\"691,-287.5 1246,-287.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"823.5\" y=\"-272.3\">quad_state</text>\n<polyline fill=\"none\" points=\"691,-264.5 956,-264.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"823.5\" y=\"-249.3\">trunk_input</text>\n<polyline fill=\"none\" points=\"956,-241.5 956,-287.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1101\" y=\"-272.3\">quad_torques</text>\n<polyline fill=\"none\" points=\"956,-264.5 1246,-264.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1101\" y=\"-249.3\">output_metrics</text>\n</g>\n<!-- 93882169739536&#45;&gt;93882193386624 -->\n<g class=\"edge\" id=\"edge4\">\n<title>93882169739536:y1-&gt;93882193386624:u0</title>\n<path d=\"M655,-470C738.93,-470 614.87,-292.31 680.92,-277.04\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"681.41,-280.51 691,-276 680.69,-273.55 681.41,-280.51\" stroke=\"black\"/>\n</g>\n<!-- 93882169716944&#45;&gt;93882169739536 -->\n<g class=\"edge\" id=\"edge2\">\n<title>93882169716944:y1-&gt;93882169739536:u0</title>\n<path d=\"M1015.5,-457C1015.5,-293.74 815.9,-499.88 655,-527.5 585.84,-539.37 73.56,-576.67 23.5,-527.5 8.79,-513.05 0.73,-483.98 12.9,-474.82\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"13.89,-478.18 22.5,-472 11.92,-471.46 13.89,-478.18\" stroke=\"black\"/>\n</g>\n<!-- 93882194149920 -->\n<g class=\"node\" id=\"node5\">\n<title>93882194149920</title>\n<polygon fill=\"none\" points=\"1282,-482 1282,-528 1443,-528 1443,-482 1282,-482\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1362.5\" y=\"-512.8\">meshcat_visualizer</text>\n<polyline fill=\"none\" points=\"1282,-505 1443,-505 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1352\" y=\"-489.8\">lcm_visualization</text>\n<polyline fill=\"none\" points=\"1422,-482 1422,-505 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1432.5\" y=\"-489.8\"> </text>\n</g>\n<!-- 93882169716944&#45;&gt;93882194149920 -->\n<g class=\"edge\" id=\"edge6\">\n<title>93882169716944:y0-&gt;93882194149920:u0</title>\n<path d=\"M1086.5,-493C1169.83,-493 1193.22,-493 1271.63,-493\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"1272,-496.5 1282,-493 1272,-489.5 1272,-496.5\" stroke=\"black\"/>\n</g>\n<!-- 93882192158064 -->\n<g class=\"node\" id=\"node3\">\n<title>93882192158064</title>\n<polygon fill=\"none\" points=\"134,-24.5 134,-93.5 545,-93.5 545,-24.5 134,-24.5\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-78.3\">planners.towr.TowrTrunkPlanner@00005562a6b6dd70</text>\n<polyline fill=\"none\" points=\"134,-70.5 545,-70.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"208\" y=\"-43.8\"> </text>\n<polyline fill=\"none\" points=\"282,-24.5 282,-70.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"413.5\" y=\"-55.3\">trunk_trajectory</text>\n<polyline fill=\"none\" points=\"282,-47.5 545,-47.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"413.5\" y=\"-32.3\">trunk_geometry</text>\n</g>\n<!-- 93882192158064&#45;&gt;93882193386624 -->\n<g class=\"edge\" id=\"edge5\">\n<title>93882192158064:y0-&gt;93882193386624:u1</title>\n<path d=\"M546.5,-59C598.71,-59 621.08,-64.32 655,-104 696.76,-152.85 631.14,-242.13 680.77,-252.09\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"680.73,-255.6 691,-253 681.35,-248.63 680.73,-255.6\" stroke=\"black\"/>\n</g>\n<!-- 93882193386624&#45;&gt;93882169739536 -->\n<g class=\"edge\" id=\"edge3\">\n<title>93882193386624:y0-&gt;93882169739536:u3</title>\n<path d=\"M1247.5,-276C1263.06,-276 1257.66,-300.15 1246.5,-311 1069.45,-483.07 880.51,-469.24 691,-311 622.18,-253.53 723.82,-169.97 655,-112.5 641.53,-101.26 35.94,-100.13 23.5,-112.5 12.43,-123.51 6.23,-236.34 16.24,-267.86\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"13.63,-270.21 22.5,-276 19.17,-265.94 13.63,-270.21\" stroke=\"black\"/>\n</g>\n</g>\n</svg>"
     },
     "metadata": {},
     "execution_count": 5
    }
   ],
   "source": [
    "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
   ]
  },
  {
   "source": [
    "## 运行仿真"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f4cb48380b0>"
      ]
     },
     "metadata": {},
     "execution_count": 9
    }
   ],
   "source": [
    "# Simulator setup\n",
    "simulator = Simulator(diagram, diagram_context)\n",
    "if use_lcm:\n",
    "    # If we're using LCM to send messages to another simulator or a real\n",
    "    # robot, we don't want Drake to slow things down, so we'll publish\n",
    "    # new messages as fast as possible\n",
    "    simulator.set_target_realtime_rate(0.0)\n",
    "else:\n",
    "    simulator.set_target_realtime_rate(target_realtime_rate)\n",
    "    \n",
    "# Set initial states\n",
    "plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)\n",
    "\n",
    "q0 = np.asarray([ 1.0, 0.0, 0.0, 0.0,     # base orientation\n",
    "                  0.0, 0.0, 0.3,          # base position\n",
    "                  0.0, 0.0, 0.0, 0.0,     # ad/ab\n",
    "                 -0.8,-0.8,-0.8,-0.8,     # hip\n",
    "                  1.6, 1.6, 1.6, 1.6])    # knee\n",
    "qd0 = np.zeros(plant.num_velocities())\n",
    "plant.SetPositions(plant_context,q0)\n",
    "plant.SetVelocities(plant_context,qd0)\n",
    "\n",
    "\n",
    "# Run the simulation a bit\n",
    "simulator.AdvanceTo(0.01)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "output_type": "error",
     "ename": "AssertionError",
     "evalue": "",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mAssertionError\u001b[0m                            Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-11-c61fa01323fb>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0;31m# Run the simulation!\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 2\u001b[0;31m \u001b[0msimulator\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mAdvanceTo\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0msim_time\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[0;32m/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/anymal_control/controllers/basic_controller.py\u001b[0m in \u001b[0;36mDoSetControlTorques\u001b[0;34m(self, context, output)\u001b[0m\n\u001b[1;32m    302\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    303\u001b[0m         \u001b[0;31m# Compute controls to apply\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 304\u001b[0;31m         \u001b[0mu\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mControlLaw\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mcontext\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mq\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mv\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    305\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    306\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0muse_lcm\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/anymal_control/controllers/inverse_dynamics_controller.py\u001b[0m in \u001b[0;36mControlLaw\u001b[0;34m(self, context, q, v)\u001b[0m\n\u001b[1;32m    222\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    223\u001b[0m         \u001b[0mresult\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msolver\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mSolve\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmp\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 224\u001b[0;31m         \u001b[0;32massert\u001b[0m \u001b[0mresult\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mis_success\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    225\u001b[0m         \u001b[0mtau\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mresult\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mGetSolution\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtau\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    226\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mAssertionError\u001b[0m: "
     ]
    }
   ],
   "source": [
    "# Run the simulation!\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ]
}